#include <math.h>
#include <string.h>
#include "mex.h"

#define N4  4

void myprint(int val)
{
   printf("the number is %d\n", val);
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
   int      i, j, k, s, t, rows, cols, rr, cc, npixels, numnbr, gapval, ifkey;
   int      dim[2], keysize, u, v, **img, *nbrmat, *keynbr, *keylnbr, *keypnts;
   double   *pimg;
   
   pimg = mxGetPr(prhs[0]);
   npixels = mxGetNumberOfElements(prhs[0]);
   rows = (int) mxGetScalar(prhs[1]);
   cols = (int) mxGetScalar(prhs[2]);
   numnbr = (int) mxGetScalar(prhs[3]);
   gapval = (int) mxGetScalar(prhs[4]);
   ifkey = (int) mxGetScalar(prhs[5]);

   keysize = ceil((rows+0.0)/(gapval+0.0))*ceil((cols+0.0)/(gapval+0.0));
   img = (int **) mxMalloc(rows*sizeof(int));
   for(i=0;i<rows;i++) {
      img[i] = (int *) mxMalloc(cols*sizeof(int));
   }
  
   //printf("keysize=%d\n", keysize);
   myprint(keysize);
   dim[0] = keysize;
   plhs[0] = mxCreateNumericArray(1, dim, mxINT32_CLASS, mxREAL);
   dim[1] = keysize;
   dim[0] = N4;
   plhs[1] = mxCreateNumericArray(2, dim, mxINT32_CLASS, mxREAL);
   dim[0] = numnbr;
   plhs[2] = mxCreateNumericArray(2, dim, mxINT32_CLASS, mxREAL);
   dim[1] = npixels;
   dim[0] = numnbr;
   plhs[3] = mxCreateNumericArray(2, dim, mxINT32_CLASS, mxREAL);
   keypnts = (int *) mxGetPr(plhs[0]);
   keynbr = (int *) mxGetPr(plhs[1]);
   keylnbr = (int *) mxGetPr(plhs[2]);
   nbrmat = (int *) mxGetPr(plhs[3]);

   memset((char *)keypnts, 0, keysize*sizeof(int));
   memset((char *)keynbr, 0, keysize*N4*sizeof(int));
   memset((char *)keylnbr, 0, keysize*numnbr*sizeof(int));
   memset((char *)nbrmat, 0, npixels*numnbr*sizeof(int));

   k = 1;
   for(j=0;j<cols;j++) {
      for(i=0;i<rows;i++) {
         img[i][j] = k;
         k++;
      }
   }

   if(ifkey) {
      s = 0;
      for(j=0;j<cols;j=j+gapval) {
         for(i=0;i<rows;i=i+gapval) {
            keypnts[s] = img[i][j];
            t = 0;
            if((i-gapval) >= 0) {
               keynbr[t+s*N4] = img[i-gapval][j];
            }
            t++;
            if((j-gapval) >= 0) {
               keynbr[t+s*N4] = img[i][j-gapval];
            }
            t++;
            if((i+gapval) < rows) {
               keynbr[t+s*N4] = img[i+gapval][j];
            }
            t++;
            if((j+gapval) < cols) {
               keynbr[t+s*N4] = img[i][j+gapval];
            }
            s++;
         }
      }
   }

   u = 0;
   k = (int) floor(sqrt(numnbr+1)/2);
   for(j=0;j<cols;j=j+gapval) {
      for(i=0;i<rows;i=i+gapval) {
         v = 0;
         for(s=-k;s<k+1;s++) {
            for(t=-k;t<k+1;t++) {
               if((s==0) && (t==0))
                  continue;
               rr = i + s;
               cc = j + t;
               if(rr>=0 && rr<rows && cc>=0 && cc<cols)
                  keylnbr[u*numnbr+v] = img[rr][cc];
               v++;
            }
         }
         u++;
      }
   }

   u = 0;
   for(j=0;j<cols;j++) {
      for(i=0;i<rows;i++) {
         v = 0;
         for(s=-k;s<k+1;s++) {
            for(t=-k;t<k+1;t++) {
               if((s==0) && (t==0))
                  continue;
               rr = i + s;
               cc = j + t;
               if(rr>=0 && rr<rows && cc>=0 && cc<cols)
                  nbrmat[u*numnbr+v] = img[rr][cc];
               v++;
            }
         }
         u++;
      }
   }

   for(i=0;i<rows;i++)
      mxFree(img[i]);
   mxFree(img);
}
